from mazebuild import *
from plotmaze import *
from sparseObstacle import sparseObstacle
from RRT import *
from RRT_star import *
from RRT_new_try import *

if __name__ == '__main__':
    # 地图信息
    startPoint = (0, 0)
    endPoint = (99, 99)  # 人为划定起始点与终点
    length = 100
    width = 100  # 地图大小
    obstacleNum = 20  # 障碍物数量
    mostR = 10  # 障碍物最大半径
    minR = 3
    mostAngle = 45  # 最大转角


    #RRT算法参数
    step = 5
    Pg = 0.7
    sigma = 10

    pathList = []

    obstacle = sparseObstacle(obstacleNum, length, width, startPoint, endPoint, mostR, minR)
    plotmaze(obstacle, startPoint, endPoint, length, width)

    #rrt = RRT(obstacle, Node(point(startPoint[0], startPoint[1])), Node(point(endPoint[0], endPoint[1])), length, width, step, mostAngle, Pg)
    #rrt = RRTstar(obstacle, Node(point(startPoint[0], startPoint[1])), Node(point(endPoint[0], endPoint[1])), length, width, step, sigma)
    rrt = RRTnewtry(obstacle, Node(point(startPoint[0], startPoint[1])), Node(point(endPoint[0], endPoint[1])), length, width, step, sigma)

    path = rrt.run()
    leaf = rrt.leaf
    if not path:
        print("该地图未找到路径")
    else:
        for i in path:
            pathList.append((i.point.x, i.point.y))
        plotB(obstacle, pathList, startPoint, endPoint, length, width)
        plotAnimation(obstacle, leaf, point(0,0), point(0,0), point(startPoint[0], startPoint[1]), point(endPoint[0], endPoint[1]), length, width)
